/**
 * @file
 * @brief Defines the canMsg.
 */

#include "canbus/message.h"

namespace canbus {

CanMsg::CanMsg() {
  id_ = 0;
  data_length_ = 8;
  data_ = new uint8_t[data_length_];
}

CanMsg::CanMsg(uint8_t* value) {
  id_ = 0;
  data_length_ = 8;
  data_ = value;
}

void CanMsg::SetByte(const uint8_t& data, int digit) {
  if (digit >= 0 && digit < data_length_) {
    *(data_+digit) = data;
  }
}

void CanMsg::Reset() {
  data_ = new uint8_t[data_length_];
}

int32_t CanMsg::ParseTwins(const std::uint8_t low_byte,
                    const std::uint8_t high_byte) const {
  Byte frame_high(&high_byte);
  int high = frame_high.get_byte(0, 8);
  Byte frame_low(&low_byte);
  int low = frame_low.get_byte(0, 8);
  int32_t value = (high << 8) | low;
  return value;
}

int32_t CanMsg::ParseQuarter(const std::uint8_t q1,
                    const std::uint8_t q2,
                    const std::uint8_t q3,
                    const std::uint8_t q4) const {
  Byte frame1(&q1);
  int v1 = frame1.get_byte(0, 8);
  Byte frame2(&q2);
  int v2 = frame2.get_byte(0, 8);
  Byte frame3(&q3);
  int v3 = frame3.get_byte(0, 8);
  Byte frame4(&q4);
  int v4 = frame4.get_byte(0, 8);
  int32_t value = (v4 << 24) | (v3 << 16) | (v2 << 8) | v1;
  return value;
}

uint8_t CanMsg::getTwin(const int32_t& twins, bool is_upper) {
  if (is_upper) {
    return twins & 0xff; 
  } else {
    return (twins >> 8) & 0xff;
  }
}

uint8_t CanMsg::getQuarter(const int32_t& quarters, int position) {
  switch (position) {
    case 0:
      return quarters & 0xff;
    case 1:
      return (quarters >> 8) & 0xff;
    case 2:
      return (quarters >> 16) & 0xff;
    case 3:
      return (quarters >> 24) & 0xff;
    default:
      return 0;
  }
}

void CanMsg::UpUpdate(uint8_t* value) {
  data_ = value;
  UpUpdateData();
}

} // namespace